#ifndef VISUALIZER_IMPL_H
#define VISUALIZER_IMPL_H

#include <libplayerc++/playerc++.h>
#include <boost/shared_array.hpp>
#include "ompl_includes.h"
#include "Visualizer.h"

#include <iostream>

class VisualizerImpl : public Visualizer {
	typedef ompl::base::SE2StateSpace::StateType SE2StateType;
	PlayerCc::Graphics2dProxy &g;
	const PlayerCc::Position2dProxy &pp;

	// set color based on probability
	void setColor(double p) {
		uint8_t red = (p < 0.5? 200: 0);
		uint8_t green = (p >= 0.5? 100: 0);
		g.Color(red, green, 0, 200);
	}

public:
	VisualizerImpl(PlayerCc::Graphics2dProxy &g, const PlayerCc::Position2dProxy &pp)
	: g(g), pp(pp) {}

	void draw(double p, const ompl::geometric::PathGeometric & path) {
		std::size_t stateCount = path.getStateCount();
		boost::shared_array<player_point_2d_t> points(new player_point_2d_t[stateCount]);

		// relativize
		double ox = pp.GetXPos();
		double oy = pp.GetYPos();
		for (int i = 0; i < stateCount; ++i) {
			const SE2StateType * se2state = path.getState(i)->as<SE2StateType>();
			points[i].px = se2state->getX() - ox;
			points[i].py = se2state->getY() - oy;
		}

		// draw
		setColor(p);
		g.DrawPolyline(points.get(), stateCount);
	}

	void clear() {
		g.Clear();
	}
};

#endif
